#ifndef _MOYINGMAP_VIZ_H_
#define _MOYINGMAP_VIZ_H_

#include <dispatch_system/moying_map.h>
#include <dispatch_system/vehicle.h>
#include <dispatch_system/time_based_astar.h>
#include <ros/ros.h>
#include <nav_msgs/OccupancyGrid.h>
#include <visualization_msgs/Marker.h>
#include <visualization_msgs/MarkerArray.h>
#include <geometry_msgs/PoseArray.h>
#include <geometry_msgs/PolygonStamped.h>

using namespace moying::navigation;

namespace moying
{
namespace visualization
{
    class MoyingMapViz
    {
        public:
            MoyingMapViz(const MoyingMapPtr &moyingmap, const std::string &global_frame="map");
            virtual ~MoyingMapViz();

            void publishTimedMap(std::mutex& time_map_mutex,int time_long=0);
            void publishCostmapCoordinate();
            void publishMapCell(double length=1.000,double width=0.61800);
            void publishCorrectedCell(const std::vector<CorrectCellInfo>& cci_vec,float length,float width);
            void publishCollidePolygon(float x,float y,float theta,float length,float width);

        private:
            void prepareRawGrid(nav_msgs::OccupancyGrid &grid);
            void prepareGrid(nav_msgs::OccupancyGrid &grid);
            void onNewCostmapSubscription(const ros::SingleSubscriberPublisher& pub);
            void onNewRawCostmapSubscription(const ros::SingleSubscriberPublisher& pub);
            void timerUpdateCallback(const ros::TimerEvent& event);
            std_msgs::ColorRGBA getColorMsg(const MapObjectPtr &object);
            ros::NodeHandle nh_;
            std::string global_frame_;
            ros::Publisher raw_costmap_pub_;
            ros::Publisher costmap_pub_;
            ros::Publisher costmap_coordinate_pub_;
            ros::Publisher map_cell_pub_;
            ros::Publisher corrected_cell_pub_;
            ros::Publisher polygon_pub_[5];
            ros::Publisher timedmap_pub_;
            ros::Timer pub_timer_;
            ros::Rate timer_rate_;
            MoyingMapPtr moyingmap_;
    };
    typedef std::shared_ptr<MoyingMapViz> MoyingMapVizPtr;
}
}
#endif //_MOYINGMAP_VIZ_H_